package avl.sv.client.tools;

import avl.sv.client.MouseActionLogger;
import avl.sv.client.SlideViewer;
import avl.sv.shared.Annotations.ROIPoly;
import avl.sv.shared.sources.SlideSource;
import avl.tiff.TiffDirectoryBuffer;
import ij.gui.MessageDialog;
import java.awt.Color;
import java.awt.Dialog;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Point;
import java.awt.Rectangle;
import java.awt.Window;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.MouseEvent;
import java.awt.geom.AffineTransform;
import java.awt.geom.PathIterator;
import java.awt.image.BufferedImage;
import java.awt.image.DataBuffer;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Iterator;
import java.util.Map;
import java.util.SortedMap;
import java.util.TreeMap;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.TimeUnit;
import java.util.logging.Level;
import java.util.logging.Logger;
import javax.media.jai.JAI;
import javax.media.jai.KernelJAI;
import javax.media.jai.PlanarImage;
import javax.media.jai.operator.FormatDescriptor;
import javax.swing.AbstractAction;
import javax.swing.JMenuItem;
import javax.swing.JPopupMenu;
import javax.swing.SwingUtilities;

/**
 *
 * @author benbryan
 */
public class IntelligentScissorsTool extends AbstractSlideViewerTool {

    private final SlideViewer slideViewer;
    private final SlideSource slideSource;
    private TiffDirectoryBuffer workingDir;
    private ROIPoly fixedROI = null;
    private ROIPoly dynamicROI = null;
    private MouseActionLogger mouseAction = new MouseActionLogger();
    private Point startPoint;
    private IntelligentScissorsWeights weights = new IntelligentScissorsWeights();
    private boolean isWorkOnBaseResolution, isApproxCosineInvserse, isDemo;
    
    private Point start_node;
    private int M, N, last_col, last_row;
    private Dist dist;
    private Path path;

    private final int PADDING = 5, COST_DIM = 100;
    private final int PADDED_DIM = COST_DIM + 2 * PADDING;
    private float costM[] = new float[8 * COST_DIM * COST_DIM];
    
    private final float gradData[] = new float[PADDED_DIM * PADDED_DIM];
    private final float dxData[] = new float[PADDED_DIM * PADDED_DIM];
    private final float dyData[] = new float[PADDED_DIM * PADDED_DIM];   
    
    private final byte g5Cross[] = new byte[PADDED_DIM*PADDED_DIM];
    private final byte g9Cross[] = new byte[PADDED_DIM*PADDED_DIM];
    
    PlanarImage imgFloat;
    
    
    private final static float mask5x5[] = {0.009345855f, 0.046901669f, 0.074026778f, 0.046901669f, 0.009345855f,
        0.046901669f, 0.093097717f, -0.076360062f, 0.093097717f, 0.046901669f,
        0.074026778f, -0.076360062f, -0.77565455f, -0.076360062f, 0.074026778f,
        0.046901669f, 0.093097717f, -0.076360062f, 0.093097717f, 0.046901669f,
        0.009345855f, 0.046901669f, 0.074026778f, 0.046901669f, 0.009345855f};

    private final static float mask9x9[] = {5.59E-05f, 0.000232978f, 0.00091235f, 0.002088718f, 0.002736173f, 0.002088718f, 0.00091235f, 0.000232978f, 5.59E-05f,
        0.000232978f, 0.001588481f, 0.005942293f, 0.011815447f, 0.014364959f, 0.011815447f, 0.005942293f, 0.001588481f, 0.000232978f,
        0.00091235f, 0.005942293f, 0.017000664f, 0.01994336f, 0.01491824f, 0.01994336f, 0.017000664f, 0.005942293f, 0.00091235f,
        0.002088718f, 0.011815447f, 0.01994336f, -0.023389196f, -0.070777208f, -0.023389196f, 0.01994336f, 0.011815447f, 0.002088718f,
        0.002736173f, 0.014364959f, 0.01491824f, -0.070777208f, -0.15347332f, -0.070777208f, 0.01491824f, 0.014364959f, 0.002736173f,
        0.002088718f, 0.011815447f, 0.01994336f, -0.023389196f, -0.070777208f, -0.023389196f, 0.01994336f, 0.011815447f, 0.002088718f,
        0.00091235f, 0.005942293f, 0.017000664f, 0.01994336f, 0.01491824f, 0.01994336f, 0.017000664f, 0.005942293f, 0.00091235f,
        0.000232978f, 0.001588481f, 0.005942293f, 0.011815447f, 0.014364959f, 0.011815447f, 0.005942293f, 0.001588481f, 0.000232978f,
        5.59E-05f, 0.000232978f, 0.00091235f, 0.002088718f, 0.002736173f, 0.002088718f, 0.00091235f, 0.000232978f, 5.59E-05f};

    private final KernelJAI gabor5x5 = new KernelJAI(5, 5, mask5x5);
    private final KernelJAI gabor9x9 = new KernelJAI(9, 9, mask9x9);

    private final KernelJAI vert = KernelJAI.GRADIENT_MASK_SOBEL_VERTICAL;
    private final KernelJAI horz = KernelJAI.GRADIENT_MASK_SOBEL_HORIZONTAL;

    private final class Dist {
        private final int tileSize = 128;
        private final ArrayList<ArrayList<float[][]>> xTiles;

        public Dist(int m, int n, int rowIdx, int colIdx) {
            int numOfXtiles = (int) Math.ceil((float) m / tileSize);
            int numOfYtiles = (int) Math.ceil((float) n / tileSize);
            xTiles = new ArrayList<>();
            for (int x = 0; x < numOfXtiles; x++) {
                ArrayList<float[][]> temp = new ArrayList<>();
                for (int y = 0; y < numOfYtiles; y++) {
                    temp.add(null);
                }
                xTiles.add(temp);
            }
            set(rowIdx, colIdx, 0);
        }

        private float[][] newTile() {
            float[][] tile = new float[tileSize][tileSize];
            for (int ix = 0; ix < tileSize; ix++) {
                for (int iy = 0; iy < tileSize; iy++) {
                    tile[ix][iy] = Float.MAX_VALUE;
                }
            }
            return tile;
        }

        public void set(int x, int y, float v) {
            int xIdx = (int) Math.floor((float) x / tileSize);
            int yIdx = (int) Math.floor((float) y / tileSize);
            ArrayList<float[][]> yTiles = xTiles.get(xIdx);
            float tile[][] = yTiles.get(yIdx);
            if (tile == null) {
                tile = newTile();
                yTiles.set(yIdx, tile);
            }
            tile[x - xIdx * tileSize][y - yIdx * tileSize] = v;
        }

        public float get(int x, int y) {
            int xIdx = (int) Math.floor((float) x / tileSize);
            int yIdx = (int) Math.floor((float) y / tileSize);
            ArrayList<float[][]> yTiles = xTiles.get(xIdx);
            float tile[][] = yTiles.get(yIdx);
            if (tile == null) {
                tile = newTile();
                yTiles.set(yIdx, tile);
            }
            return tile[x - xIdx * tileSize][y - yIdx * tileSize];
        }

    }

    private final class Path {

        private final int m, n;
        private final int tileSize = 128;
        private final ArrayList<ArrayList<Point[][]>> xTiles;
        
        public Path(int m, int n) {
            this.m = m;
            this.n = n;
            int numOfXtiles = (int) Math.ceil((float) m / tileSize);
            int numOfYtiles = (int) Math.ceil((float) n / tileSize);
            xTiles = new ArrayList<>();
            for (int x = 0; x < numOfXtiles; x++) {
                ArrayList<Point[][]> temp = new ArrayList<>();
                for (int y = 0; y < numOfYtiles; y++) {
                    temp.add(null);
                }
                xTiles.add(temp);
            }
        }

        public int getM() {
            return m;
        }

        public int getN() {
            return n;
        }

        public int getTileSize() {
            return tileSize;
        }
        
        public boolean isInBounds(Point p) {
            return (p.x>=0) && (p.x<m) && (p.y>=0) && (p.y<n);
        }

        private Point[][] newTile() {
            Point[][] tile = new Point[tileSize][tileSize];
            for (int ix = 0; ix < tileSize; ix++) {
                for (int iy = 0; iy < tileSize; iy++) {
                    tile[ix][iy] = new Point();
                }
            }
            return tile;
        }

        public void set(Point src, Point dest) {
            int xIdx = (int) Math.floor((float) src.x / tileSize);
            int yIdx = (int) Math.floor((float) src.y / tileSize);
            ArrayList<Point[][]> yTiles = xTiles.get(xIdx);
            Point tile[][] = yTiles.get(yIdx);
            if (tile == null) {
                tile = newTile();
                yTiles.set(yIdx, tile);
            }
            tile[src.x - xIdx * tileSize][src.y - yIdx * tileSize] = dest;
        }

        public Point get(Point p) {
            int xIdx = (int) Math.floor((float) p.x / tileSize);
            int yIdx = (int) Math.floor((float) p.y / tileSize);
            ArrayList<Point[][]> yTiles = xTiles.get(xIdx);
            Point tile[][] = yTiles.get(yIdx);
            if (tile == null) {
                tile = newTile();
                yTiles.set(yIdx, tile);
            }
            return tile[p.x - xIdx * tileSize][p.y - yIdx * tileSize];
        }

        public boolean isactive(Point p) {
            if (xTiles == null){
                return false;
            }
            if (p.x >= xTiles.size()){
                return false;
            }
            ArrayList<Point[][]> yTiles = xTiles.get(p.x);
            if (yTiles == null){
                return false;
            }
            if (p.y >= yTiles.size()){
                return false;
            }
            Point tile[][] = yTiles.get(p.y);
            if (tile == null){
                return false;
            } else {
                return true;
            }
        }
        
        public void inc(Point p) {
            int xIdx = (int) Math.floor((float) p.x / tileSize);
            int yIdx = (int) Math.floor((float) p.y / tileSize);
            ArrayList<Point[][]> yTiles = xTiles.get(xIdx);
            Point tile[][] = yTiles.get(yIdx);
            if (tile == null) {
                tile = newTile();
                yTiles.set(yIdx, tile);
            }
            tile[p.x - xIdx * tileSize][p.y - yIdx * tileSize].x += 1;
        }
    }

    IntelligentScissorsToolPrompt intelligentScissorsToolPrompt;
    public IntelligentScissorsTool(SlideViewer slideViewer) {
        this.slideViewer = slideViewer;
        this.slideSource = slideViewer.getSlideSource();
        setToolTipText("IntelligentScissors Tool");
        setIcon(new javax.swing.ImageIcon(getClass().getResource("/avl/sv/client/icon/scissors.png")));
        Window parentWindow = SwingUtilities.windowForComponent(this);
        intelligentScissorsToolPrompt = new IntelligentScissorsToolPrompt(parentWindow, Dialog.ModalityType.APPLICATION_MODAL);
        final JPopupMenu menu = new JPopupMenu();
        final JMenuItem jMenuItemSetup = new JMenuItem("Setup");
        final IntelligentScissorsTool thisButton = this; 
        jMenuItemSetup.addActionListener(new AbstractAction() {
            @Override
            public void actionPerformed(ActionEvent e) {
                if (fixedROI != null){
                    new MessageDialog(null, "Warning", "Cannot adjust settings with active segment");
                    return;
                }
                for (ActionListener listener:thisButton.getActionListeners()){
                    listener.actionPerformed(new ActionEvent(this, ActionEvent.ACTION_PERFORMED, "popupmenu shown"));
                }
                intelligentScissorsToolPrompt.setVisible(true);
                weights = intelligentScissorsToolPrompt.weights;
                isWorkOnBaseResolution = intelligentScissorsToolPrompt.isWorkOnBaseResolution();
                isApproxCosineInvserse = intelligentScissorsToolPrompt.isApproximateCosInv();
                isDemo = intelligentScissorsToolPrompt.isDemo();
                
            }
        });
        menu.add(jMenuItemSetup);
        jMenuItemSetup.setVisible(true);
        setComponentPopupMenu(menu);       
    }

    @Override
    public void mouseClicked(MouseEvent e) {
        if (!e.getSource().equals(slideViewer)) {
            return;
        }
        if (e.getButton() != MouseEvent.BUTTON1) {
            return;
        }
        mouseAction.mouseClicked(e);

    }

    private void appendDynamicToFixed() {
        if (dynamicROI == null){
            return;
        }
        PathIterator iter = dynamicROI.getShape().getPathIterator(new AffineTransform());
        ArrayList<float[]> pts = new ArrayList<>();
        while (!iter.isDone()) {
            float pt[] = new float[6];
            iter.currentSegment(pt);
            pts.add(pt);
            iter.next();
        }
        Collections.reverse(pts);
        for (float[] pt : pts) {
            fixedROI.addPoint(pt[0], pt[1]);
        }
    }

    private void closeROI() {
        slideViewer.getRoiManager().addROI(fixedROI);
        fixedROI = null;
        dynamicROI = null;
        slideViewer.getToolPanel().setDefaultTool();
    }

    @Override
    public void mouseMoved(MouseEvent e) {
        mouseAction.mouseMoved(e);
        Point pImg = slideViewer.displaySpaceToImageSpace(e.getPoint());
        slideViewer.getRoiManager().setRoisModified(true);
        int ix = (int) pImg.getX();
        int iy = (int) pImg.getY();
        if (fixedROI != null) {
            ROIPoly dynamicPathTemp = ROIPoly.getDefault();
            Point[] pointPath = dijkstraMain((int) (ix * workingDir.getZoomLevel()), (int) (iy * workingDir.getZoomLevel()), false);
            if (pointPath == null) {
                return;
            }
            for (Point p:pointPath) {
                dynamicPathTemp.addPoint(p.x / workingDir.getZoomLevel(), p.y / workingDir.getZoomLevel());
            }
            dynamicROI = dynamicPathTemp;
        }
    }

    @Override
    public void mouseDragged(MouseEvent e) {
        if (!e.getSource().equals(slideViewer)) {
            return;
        }
        mouseAction.mouseDragged(e);
        if (mouseAction.getMouseButton1().equals(MouseActionLogger.ButtonStatus.RELEASED)) {
            return;
        }
        if (fixedROI != null) {
            Point pImg = slideViewer.displaySpaceToImageSpace(e.getPoint());
            int ix = (int) pImg.getX();
            int iy = (int) pImg.getY();
            appendDynamicToFixed();
            dynamicROI = null;
            fixedROI.addPoint(ix, iy);
        }

    }

    @Override
    public void mousePressed(MouseEvent e) {
        if (!e.getSource().equals(slideViewer)) {
            return;
        }
        if (e.getButton() != MouseEvent.BUTTON1) {
            return;
        }
        mouseAction.mousePressed(e);
    }

    @Override
    public void mouseEntered(MouseEvent e) {  }

    @Override
    public void mouseExited(MouseEvent e) {  }

    @Override
    public void paintOnSlideViewer(Graphics2D g) {
        Point p = mouseAction.getLastMouseLocation();
        if (isDemo){
            if ((fixedROI != null) && (dynamicROI != null)){
                g.drawRect(p.x-path.tileSize/2, p.y-path.tileSize/2, path.tileSize, path.tileSize);
            }
        }    
        if (fixedROI != null) {
            slideViewer.concatenateImageToDisplayTransform(g);
            fixedROI.paintROI(g, Color.GREEN);
        }
        if (dynamicROI != null) {
            dynamicROI.paintROI(g, Color.GREEN);
        }
        
        if (isDemo){
            if ((fixedROI != null) && (dynamicROI != null)){
                if (path!= null){
                    for (int x = 0; x < (int) Math.ceil((float) path.m / path.tileSize); x++) {
                        for (int y = 0; y < (int) Math.ceil((float) path.n / path.tileSize); y++) {
                            if (path.isactive(new Point(x, y))){
                                double mag = workingDir.getZoomLevel();
                                g.drawRect((int)(x/mag*path.getTileSize()), (int)(y/mag*path.getTileSize()), (int)(path.tileSize/mag), (int)(path.tileSize/mag));
                            }
                        }
                    }
                }
            }
        }
        
    }

    @Override
    public void mouseReleased(MouseEvent e) {
        if (!e.getSource().equals(slideViewer)) {
            return;
        }
        if (e.getButton() != MouseEvent.BUTTON1) {
            return;
        }
        mouseAction.mouseReleased(e);
       
        Point pImg = slideViewer.displaySpaceToImageSpace(e.getPoint());
        slideViewer.getRoiManager().setRoisModified(true);
        int ix = (int) pImg.getX();
        int iy = (int) pImg.getY();

        if (fixedROI == null) {
            fixedROI = ROIPoly.getDefault();
            startPoint = new Point(ix, iy);
            //Iterates through the tiff dirs to find the lowest resolution in view
            if (isWorkOnBaseResolution){
                workingDir = slideSource.getBaseDirectoryBuffer();
            } else {
                TreeMap<Double, TiffDirectoryBuffer> treeMap = slideSource.getZoomMap();
                Map.Entry<Double, TiffDirectoryBuffer> thumbnail = treeMap.firstEntry();
                SortedMap<Double, TiffDirectoryBuffer> m = treeMap.tailMap(thumbnail.getKey());
                Iterator<TiffDirectoryBuffer> dirIt = m.values().iterator();
                while (dirIt.hasNext()) {
                    TiffDirectoryBuffer dir = dirIt.next();
                    if (dir.getZoomLevel() <= 2 * slideViewer.getMagnification()) {
                        workingDir = dir;
                    }
                }
            }
            M = workingDir.getImageWidth();
            N = workingDir.getImageLength();
            
            int row_index = (int) (ix * workingDir.getZoomLevel());
            int col_index = (int) (iy * workingDir.getZoomLevel());
            
            if ((row_index >= M) || (col_index >= N) || (row_index < 0) || (col_index < 0)){
                fixedROI = null;
                return;
            }

            dijkstraMain(row_index, col_index, true);
        } else if (e.getClickCount() > 1) {
            dynamicROI = null;
            closeROI();
        } else if (new Point(ix, iy).distance(startPoint) < (5 / slideViewer.getMagnification())) {
            appendDynamicToFixed();
            dynamicROI = null;
            closeROI();
        }else {
            appendDynamicToFixed();
            dijkstraMain((int) (ix * workingDir.getZoomLevel()), (int) (iy * workingDir.getZoomLevel()), true);
        }
        slideViewer.repaint();
        
    }
   
    private void costMatrix(int row_index, int col_index) {
        Rectangle r = new Rectangle(row_index - COST_DIM / 2 - PADDING, col_index - COST_DIM / 2 - PADDING, PADDED_DIM, PADDED_DIM);
        BufferedImage img = slideSource.getSubImage(r, workingDir);
        img = rgb2gray(img);
        imgFloat = FormatDescriptor.create(img, DataBuffer.TYPE_FLOAT, null).createInstance();
        
        PlanarImage dx = JAI.create("Convolve", imgFloat, horz).createInstance();
        PlanarImage dy = JAI.create("Convolve", imgFloat, vert).createInstance();
        PlanarImage grad = JAI.create("GradientMagnitude", imgFloat, horz, vert);

        grad.getData().getSamples(0, 0, PADDED_DIM, PADDED_DIM, 0, gradData);
        dx.getData().getSamples(0, 0, PADDED_DIM, PADDED_DIM, 0, dyData);
        dy.getData().getSamples(0, 0, PADDED_DIM, PADDED_DIM, 0, dxData);
        
        float minG = Float.MAX_VALUE, maxG = Float.MIN_VALUE;
        for (int i = 2; i < PADDED_DIM - 2; i++) {
            for (int j = 2; j < PADDED_DIM - 2; j++) {
                float s = gradData[i + j * PADDED_DIM];
                if (s < minG) {
                    minG = s;
                }
                if (s > maxG) {
                    maxG = s;
                }
            }
        }
        
        for (int i = 0; i < gradData.length; i++) {
            if (gradData[i] < 0.00001) {
                dxData[i] = 0;
                dyData[i] = 0;
            } else {
                dxData[i] /= gradData[i];
                dyData[i] /= gradData[i];
            }
        }
        try {
            ExecutorService threadPool = Executors.newFixedThreadPool(8);
            threadPool.submit(zeroCross5x5Runnable);
            threadPool.submit(zeroCross9x9Runnable);
            threadPool.shutdown();
            threadPool.awaitTermination(2, TimeUnit.SECONDS);

            threadPool = Executors.newFixedThreadPool(8);
            final int cOff[] = {-1,  0,  1,  1,  1,  0, -1, -1 };
            final int rOff[] = {-1, -1, -1,  0,  1,  1,  1,  0 };
            for (int K = 0; K < 8; K++) {
                threadPool.submit(new CostMaxGen(K, cOff[K] , rOff[K], minG, maxG ));
            }
            threadPool.shutdown();
            threadPool.awaitTermination(2, TimeUnit.SECONDS);
        } catch (InterruptedException ex) {
            System.out.println("Failed to generate cost matrix");
            Logger.getLogger(IntelligentScissorsTool.class.getName()).log(Level.WARNING, null, ex);
        }
    }
    
    private ZeroCrossGen zeroCross5x5Runnable = new ZeroCrossGen(gabor5x5, g5Cross);
    private ZeroCrossGen zeroCross9x9Runnable = new ZeroCrossGen(gabor9x9, g9Cross);
    private class ZeroCrossGen implements Runnable{
        final KernelJAI gaborKernel;
        final byte[] cross;
        private float gaborResult[] = new float[PADDED_DIM*PADDED_DIM];

        public ZeroCrossGen(KernelJAI gaborKernel, byte[] cross) {
            this.gaborKernel = gaborKernel;
            this.cross = cross;
        }       
        
        @Override
        public void run() {
            PlanarImage g5 = JAI.create("Convolve", imgFloat, gaborKernel).createInstance();
            g5.getData().getSamples(0, 0, PADDED_DIM, PADDED_DIM, 0, gaborResult);

            double sum = 0;
            for (int i = 0; i < PADDED_DIM * PADDED_DIM; i++) {
                sum += Math.abs(gaborResult[i]);
            }
            double meanAbs = sum / (PADDED_DIM * PADDED_DIM);
            double thresh = 0.75 * meanAbs;

            for (int i = 1; i < PADDED_DIM - 1; i++) {
                for (int j = 1; j < PADDED_DIM - 1; j++) {
                    final int rOff[] = {i - 1, i, i + 1, i + 1, i + 1, i, i - 1, i - 1};
                    final int cOff[] = {j - 1, j - 1, j - 1, j, j + 1, j + 1, j + 1, j};
                    int pc = i + j * PADDED_DIM;
                    for (int K = 0; K < 4; K++) {
                        int p1 = rOff[K] + cOff[K] * PADDED_DIM;
                        int p2 = rOff[K + 4] + cOff[K + 4] * PADDED_DIM;
                        float v51 = gaborResult[p1];
                        float v52 = gaborResult[p2];
                        if ((Math.abs(v51) > thresh) && (Math.abs(v52) > thresh) && ((v51 * v52) < 0)) {
                            cross[pc] = 0;
                        } else {
                            cross[pc] = 1;
                        }
                    }
                }
            }


        }
        
    }
    
    private class CostMaxGen implements Runnable{
        final int K, cOff, rOff; 
        final float minG, maxG;

        public CostMaxGen(int K, int cOff, int rOff, float minG, float maxG) {
            this.K = K;
            this.cOff = cOff;
            this.rOff = rOff;
            this.minG = minG;
            this.maxG = maxG;
        }
        
        @Override
        public void run() {
            for (int i = PADDING; i < PADDED_DIM - PADDING; i++) {
                for (int j = PADDING; j < PADDED_DIM - PADDING; j++) {
                    //Copy matrices into memory
                    final int first_coord = i + j*PADDED_DIM;
                    final int second_coord = (cOff + i) + (rOff + j) * PADDED_DIM;
                    float L0 = cOff;
                    float L1 = rOff;
                    float factor = 1;
                    if (K % 2 == 0) {
                        L0 *= 0.70710678118f;
                        L1 *= 0.70710678118f;                        
                        factor = 1.41421356f; // 1/sqrt(2)
                    }

                    float temp1 = (dyData[first_coord] * L1 - dxData[first_coord] * L0);
                    float temp2 = (dyData[second_coord] * L1 - dxData[second_coord] * L0);
                    temp1 = Math.abs(temp1);
                    temp2 = Math.abs(temp2);
                    float fd;
                    if (isApproxCosineInvserse){
                        fd = (float) ((acos(temp1) + acos(temp2)) / Math.PI);
                    } else {
                        fd = (float) ((Math.acos(temp1) + Math.acos(temp2)) / Math.PI);
                    }
                    float fg = (1f - ((gradData[second_coord] - minG) / (maxG - minG))) * factor;
                    float fz = (float) (0.45 * g5Cross[second_coord] + 0.55 * g9Cross[second_coord]) * factor;
                    double temp = weights.g*fg + weights.d*fd+weights.z*fz;
                    temp = Math.max(temp, 0.00001f);
                    costM[((i - PADDING) + COST_DIM * (j - PADDING)) * 8 + K] = (float) temp;
                }
            }
        }
    }

    private double acos(double x) {
        return (-0.69813170079773212 * x * x - 0.87266462599716477) * x + 1.5707963267948966;
    }

    private Point[] dijkstraMain(int row_index, int col_index, boolean firstRun) {

        Point path_array[];
        if (firstRun) {
            start_node = new Point(row_index, col_index);
            path = new Path(M, N);
            dist = new Dist(M, N, row_index, col_index);
        }

        updateDist(row_index, col_index, firstRun);

        if (!firstRun) {
            if (((last_col == 0) && (last_row == 0))) {
                return null;
            } else {
                path_array = forcePath(row_index, col_index, firstRun);
                if (path_array == null) {//fail
                    return forcePath(row_index, col_index, firstRun);
                }
                last_col = col_index;
                last_row = row_index;
                return path_array;
            }
        }
        last_col = col_index;
        last_row = row_index;
        return null;
    }

    private Point[] forcePath(final int row_index, final int col_index, final boolean firstRun) {
        Point freePoint = new Point(row_index , col_index);
        Point[] path_array = new Point[5000];
        path_array[0] = freePoint;
        int path_array_index = 1;
        Point new_node = (Point) freePoint.clone();
        int[] nodes_row = new int[]{last_row, row_index};
        int[] nodes_col = new int[]{last_col, col_index};
        int nodesCount = 2;
        while (!new_node.equals(start_node)) {
            if ((path_array_index >= (50000))) {
                return null;
            }

            //If a path was not found, incrementally update distance matrix with points between start and finish node
            if (new_node.equals(new Point())) {
                if (nodesCount > 32) {
                    return null;
                }
                int[] nodesNew_row = new int[nodesCount * 2 - 1];
                int[] nodesNew_col = new int[nodesCount * 2 - 1];
                //Copy in old nodes

                for (int x = 0; x < nodesCount; x++) {
                    nodesNew_row[x * 2] = nodes_row[x];
                    nodesNew_col[x * 2] = nodes_col[x];
                }
                //Make new nodes
                for (int x = 1; x < nodesCount * 2 - 1; x += 2) {
                    nodesNew_row[x] = (int) Math.floor((float) (nodesNew_row[x - 1] + nodesNew_row[x + 1]) / 2);
                    nodesNew_col[x] = (int) Math.floor((float) (nodesNew_col[x - 1] + nodesNew_col[x + 1]) / 2);
                }
                nodes_row = nodesNew_row;
                nodes_col = nodesNew_col;
                nodesCount = nodesCount * 2 - 1;

                for (int x = 0; x < nodesCount; x++) {
                    updateDist(nodes_row[x], nodes_col[x], firstRun);
                }
                //Restart the processes for finding the path
                path_array_index = 1;
                new_node = freePoint;
            }

            if (path_array_index >= path_array.length - 1) {
                Point temp[] = new Point[path_array.length * 2];
                System.arraycopy(path_array, 0, temp, 0, path_array.length);
                path_array = temp;
                path_array[0] = freePoint;
            }

            if (path_array_index > 1000) {
                Point temp = path.get(new_node);
                for (int i = 1; i < 5; i++){
                    if (path_array[path_array_index - i].equals(temp)) {
                       path.inc(new_node);
                       System.out.println("inc");
                    }                   
                }

            }
            //TODO: remove int cast
            new_node = path.get(new_node);
            path_array[path_array_index++] = new_node;
        }

        //Create final output with only significant elements
        Point[] temp = new Point[path_array_index];
        System.arraycopy(path_array, 0, temp, 0, path_array_index);
        //This does need to be here
        return temp;
    }

    private void makeNeigh(final int i, final int j, final int row_index, final int col_index) {
        if ((i < PADDING) || (j < PADDING) || (i >= (M - PADDING)) || (j >= (N - PADDING))) {
            return;
        }
        final int rOff[] = {i - 1, i, i + 1, i + 1, i + 1, i, i - 1, i - 1};
        final int cOff[] = {j - 1, j - 1, j - 1, j, j + 1, j + 1, j + 1, j};
        final int ptIdx = ((i - row_index + COST_DIM / 2) + (j - col_index + COST_DIM / 2) * COST_DIM) * 8;
        for (int K = 0; K < 8; K++) {
            float temp = costM[ptIdx + K] + dist.get(i, j);
            if (temp < dist.get(rOff[K], cOff[K])) {
                dist.set(rOff[K], +cOff[K], temp);
            }
        }
    }

    private void updateDist(final int row_index, final int col_index, final boolean firstRun) {
        final int bound =  COST_DIM / 2;
        costMatrix(row_index, col_index);
        if (firstRun == true) {
            for (int W = 0; W < bound; W++) {
                clockWise(W, col_index, row_index);
            }
        }
        for (int W = bound - 1; W >= 0; W--) {
            clockWise(W, col_index, row_index);

        }
        for (int W = 0; W < bound; W++) {
            clockWise(W, col_index, row_index);

        }
        if (firstRun == false) {
            for (int W = bound - 1; W >= 0; W--) {
                clockWise(W, col_index, row_index);
            }
        }
        makePath(row_index, col_index);
    }

    private void clockWise(int W, int col_index, int row_index) {
        int I, J;
        J = col_index + W;
        for (I = row_index - W; I < row_index + W; I++) { //Right Side, Down
            makeNeigh(I, J, row_index, col_index);
        }
        for (J = col_index + W; J > col_index - W; J--) { //Bottom Side, To Left
            makeNeigh(I, J, row_index, col_index);
        }
        for (I = row_index + W; I > row_index - W; I--) { //Left Side, Up
            makeNeigh(I, J, row_index, col_index);
        }
        for (J = col_index - W; J < col_index + W; J++) { //Top, To Right
            makeNeigh(I, J, row_index, col_index);
        }
    }

    private void makePath(int row_index, int col_index) {
        int j, i, K, Kl;
        float min, temp;
        final int bounds = COST_DIM / 2 + PADDING;
        for (j = col_index - bounds; j < col_index + bounds; j++) {
            for (i = row_index - bounds; i < row_index + bounds; i++) {
                if ((i < 4) || (j < 4) || (i >= (M - 4)) || (j >= (N - 4))) {
                    continue;
                }
                final int rOff[] = {i - 1, i, i + 1, i + 1, i + 1, i, i - 1, i - 1};
                final int cOff[] = {j - 1, j - 1, j - 1, j, j + 1, j + 1, j + 1, j};

                min = Float.MAX_VALUE;
                Kl = -1;
                for (K = 0; K < 8; K++) {
                    temp = dist.get(rOff[K], cOff[K]);
                    if (temp < min) {
                        min = temp;
                        Kl = K;
                    }
                }
                if (Kl >= 0) {
                    path.set(new Point(i, j), new Point(rOff[Kl], cOff[Kl]));
                }
            }
        }
    }

    private BufferedImage rgb2gray(BufferedImage in) {
        BufferedImage out = new BufferedImage(in.getWidth(), in.getHeight(), BufferedImage.TYPE_BYTE_GRAY);
        Graphics g = out.getGraphics();
        g.drawImage(in, 0, 0, null);
        g.dispose();
        return in;
    }

}
